#include "Variant.h"

uint8_t VariantToRestore(C_U8* a_pu8Src, C_U8 a_u8SrcLen, C_U8* a_pu8Dest, C_U8* a_pu8DestLen)
{
    if (a_pu8Src == NULL || a_pu8Dest == NULL)
        return C_FALSE;

    if (a_pu8Src[0] != 0xaa || a_pu8Src[1] != 0xaa ||
        a_pu8Src[a_u8SrcLen - 1] != 0x55 || a_pu8Src[a_u8SrcLen - 2] != 0x55)
        return C_FALSE;

    #if 0
    printf("before restore(%d):\n", a_u8SrcLen);
    for (C_U8 i = 0; i < a_u8SrcLen; i++)
        printf("%02x ", a_pu8Src[i]);
    printf("\n");
    #endif

    C_U8 i = 0;
    a_pu8Dest[i++] = a_pu8Src[0];
    a_pu8Dest[i++] = a_pu8Src[1];

    for (C_U8 j = 2; j < a_u8SrcLen - 2; j++)
    {
        a_pu8Dest[i++] = a_pu8Src[j];
        if (a_pu8Src[j] == 0xaa || a_pu8Src[j] == 0x55)
        {
            if (a_pu8Src[++j] != 0x1b)
            {
				#if 0
                printf("exception data\n");
				#endif
                return C_FALSE;
            }
        }
    }

    a_pu8Dest[i++] = a_pu8Src[a_u8SrcLen - 2];
    a_pu8Dest[i++] = a_pu8Src[a_u8SrcLen - 1];

    *a_pu8DestLen = i;

    #if 0
    printf("after restore(%d):\n", *a_pu8DestLen);
    for (C_U8 i = 0; i < *a_pu8DestLen; i++)
        printf("%02x ", a_pu8Dest[i]);
    printf("\n");
    #endif

    return C_TRUE;
}

uint8_t RestoreToVariant(C_U8* a_pu8Src, C_U8 a_u8SrcLen, C_U8* a_pu8Dest, C_U8* a_pu8DestLen)
{
    if (a_pu8Src == NULL || a_pu8Dest == NULL)
        return C_FALSE;

    if (a_pu8Src[0] != 0xaa || a_pu8Src[1] != 0xaa ||
        a_pu8Src[a_u8SrcLen - 1] != 0x55 || a_pu8Src[a_u8SrcLen - 2] != 0x55)
        return C_FALSE;

    #if 0
    printf("before variant(%d):\n", a_u8SrcLen);
    for (C_U8 i = 0; i < a_u8SrcLen; i++)
        printf("%02x ", a_pu8Src[i]);
    printf("\n");
    #endif

    C_U8 i = 0;
    a_pu8Dest[i++] = a_pu8Src[0];
    a_pu8Dest[i++] = a_pu8Src[1];

    for (C_U8 j = 2; j < a_u8SrcLen - 2; j++)
    {
        a_pu8Dest[i++] = a_pu8Src[j];
        if (a_pu8Src[j] == 0xaa || a_pu8Src[j] == 0x55)
            a_pu8Dest[i++] = 0x1b;
    }

    a_pu8Dest[i++] = a_pu8Src[a_u8SrcLen - 2];
    a_pu8Dest[i++] = a_pu8Src[a_u8SrcLen - 1];
    *a_pu8DestLen = i;

    #if 0
    printf("after variant(%d):\n", *a_pu8DestLen);
    for (C_U8 i = 0; i < *a_pu8DestLen; i++)
        printf("%02x ", a_pu8Dest[i]);
    printf("\n");
    #endif

    return C_TRUE;
}